/*
 * findHomography.cpp
 *
 *  Created on: Aug 15, 2013
 *      Author: Daniel Becker
 *
 * This program
 */
#if 0

#include <stdio.h>
#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;


/** @function main */
int main( int argc, char** argv )
{

  //-- Step 1: Import key points from csv file
  std::vector<Point2f> objectPlane;
  std::vector<Point2f> imagePlane;

  // Input object plane measurements (in m)
  objectPlane.push_back(Point2f(5,-1.1));
  objectPlane.push_back(Point2f(6,-1.1));
  objectPlane.push_back(Point2f(7,-1.1));
  objectPlane.push_back(Point2f(8,-1.1));
  objectPlane.push_back(Point2f(9,-1.1));
  objectPlane.push_back(Point2f(10,-1.1));
  objectPlane.push_back(Point2f(11,-1.1));
  objectPlane.push_back(Point2f(12,-1.1));
  objectPlane.push_back(Point2f(13,-1.1));
  objectPlane.push_back(Point2f(14,-1.1));
  objectPlane.push_back(Point2f(15,-1.1));
  objectPlane.push_back(Point2f(5,-2.55));
  objectPlane.push_back(Point2f(11,-2.55));

  // Input image plane measurements (in Pixels)
  imagePlane.push_back(Point2f(509,448));
  imagePlane.push_back(Point2f(487,393));
  imagePlane.push_back(Point2f(467,350));
  imagePlane.push_back(Point2f(457,317));
  imagePlane.push_back(Point2f(449,293));
  imagePlane.push_back(Point2f(441,273));
  imagePlane.push_back(Point2f(437,256));
  imagePlane.push_back(Point2f(429,243));
  imagePlane.push_back(Point2f(423,230));
  imagePlane.push_back(Point2f(419,220));
  imagePlane.push_back(Point2f(415,212));
  imagePlane.push_back(Point2f(749,420));
  imagePlane.push_back(Point2f(557,251));


  printf("Object plane x/y (meters) - image plane x/y (pixels)");
  for(int i=0; i<objectPlane.size(); i++) {
	  printf("%2d: %.2f/%.2f - %.0f/%.0f\n", i, objectPlane.at(i).x, objectPlane.at(i).y, imagePlane.at(i).x, imagePlane.at(i).y);
  }
  printf("\n");


  //Mat H = findHomography( objectPlane, imagePlane, CV_LMEDS);
  //Mat H = findHomography( objectPlane, imagePlane, CV_RANSAC);
  Mat H = findHomography( objectPlane, imagePlane, 0 );
  std::cout << "H = "<< std::endl << " "  << H << std::endl << std::endl;


  // H transformation:
  Mat pixelCoord = Mat::ones(3,1,CV_64FC1);
  Mat realCoord = Mat::ones(3,1,CV_64FC1);

  pixelCoord.at<cv::Vec2d>(0,0)[0] = 478; // img x
  pixelCoord.at<cv::Vec2d>(1,0)[0] = 271; // img y

  std::cout << "pixelCoord = "<< std::endl << " "  << pixelCoord << std::endl << std::endl;

  // Calculate real from pixel coordinates
  Mat Hinv = H.inv();
  realCoord = Hinv * pixelCoord;

  // normalize and print result
  double scale = 1 / realCoord.at<cv::Vec2d>(2,0)[0];
  realCoord = realCoord*scale;
  std::cout << "realCoord = " << std::endl << " "  << realCoord << std::endl << std::endl;

  return 0;
  }

#endif
